#include <stddef.h>
#include <plib.h>
#include "system_config.h"
#include "CoreTimerDelay.h"
#include "String.h"

const unsigned int countPerMicroSec = ((SYS_FREQ/1000000)/2);
CoreTimerCallback CTcallback = NULL;

void CoreDelay_Init(){
    // Set up core time
}

void CoreDelay_Sync_us(unsigned int us){
    unsigned int targetCount;
    unsigned int backupCount;
    unsigned char loop = 0;

    // backup current count of the core timer.
    backupCount = ReadCoreTimer();
    // Core timer increments every 2 sys clock cycles.
    // Calculate the counts required to complete "us".
    targetCount = countPerMicroSec * us;
    // Restart core timer.
    WriteCoreTimer(0);
    // Wait till core timer completes the count.
    while(ReadCoreTimer() < targetCount);
    // Restore count back.
    WriteCoreTimer(backupCount + targetCount);
}

void CoreDelay_Callback(CoreTimerCallback function){
    // For now only allow one call back
    if (CTcallback != NULL && function != NULL) return;
    CTcallback = function;
}

int CoreDelay_Async_us(unsigned int us){
    // Set up the time for the call back
    mConfigIntCoreTimer((CT_INT_OFF | CT_INT_PRIOR_5 | CT_INT_SUB_PRIOR_0));
    WriteCoreTimer(0);
    _mtc0(11,0,us);
    mEnableIntCoreTimer();
}

void __ISR( _CORE_TIMER_VECTOR, ipl5) CoreInterrupt( void){
    mDisableIntCoreTimer();
    if (CTcallback != NULL){
        CTcallback();
    }
    CTcallback == NULL;
    mCTClearIntFlag();
}